from mpu6050 import Mpu6050
import time    
import numpy as np

class Alttitude():

    def __init__(self):
        self.mpu = Mpu6050(0x68)

        # ------------Calib-----------------------
        mean_gyro = np.array([0, 0, 0], dtype = np.float)
        for i in range(1000):
            mean_gyro += np.array(self.mpu.get_gyro_data())
        mean_gyro /= 1000

        self.mean_gyro = mean_gyro

        #-----------Timer--------------
        self.t = time.time()

        #---------Data buffer-------
        self.last_gyro = 0

        #---------Output-----------
        self.yaw = 0


    #---------Loop Task--------------
    def Alttitude_Measure_Task(self):

        #-------------Error Compensation---------------
        gyro_data = self.mpu.get_gyro_data() - self.mean_gyro
        time_stamp = time.time()

        #------------Low Pass Filter-------------------
        current_gyro = self.last_gyro * 0.3 + gyro_data[2] * 0.7
        self.last_gyro = current_gyro

        #------------Integration---------------------
        if abs(current_gyro) > 0.3:
            self.yaw += current_gyro * (time_stamp - self.t)

        #------------Time stamp Update--------------
        self.t = time_stamp